import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from cycler import cycler

def read(file_path = '附件5：P1-P2的行驶路径.xlsx'):
    df = pd.read_excel(file_path)
    array = df[['栅格x坐标', '栅格y坐标','车头朝向（单位：度）']].values.tolist()
    return array

def phi(m1, m2):
    S1, A1 = m1[1], m1[2]
    S1/=180/np.pi
    A1/=180/np.pi
    n = np.array([np.sin(S1) * np.sin(A1), np.sin(S1) * np.cos(A1), np.cos(S1)])
    S2, A2 = m2[1], m2[2]
    S2/=180/np.pi
    A2/=180/np.pi
    npre = np.array([np.sin(S2) * np.sin(A2), np.sin(S2) * np.cos(A2), np.cos(S2)])
    temp = np.dot(n, npre) / np.linalg.norm(n) / np.linalg.norm(npre)
    return np.arccos(temp) if temp <= 1 else 0

omega = {1:{0:1,45:1.5,90:2}, 2:{0:np.sqrt(2),45:np.sqrt(2)+0.5,90:np.sqrt(2)+1}}
def v(x):
    if x < 10: return 30*1000/3600
    elif x < 20: return 20*1000/3600
    elif x < 30: return 10*1000/3600
    else: return 0

def go(M):
    data = read()# 缺省以计算第一问
    D = T = stable = T_bad = 0
    path_coordinates = []
    Dset = []
    for i in range(1, len(data)):
        x, y = data[i][0], data[i][1]
        xp, yp = data[i-1][0], data[i-1][1]
        dL = abs(y-yp) + abs(x-xp)
        dtheta = abs(data[i][2] - data[i-1][2])
        dtheta = min(dtheta, 360-dtheta)
        D += omega[dL][dtheta] * 5
        if M[x,y,1] != 0:
            T += omega[dL][dtheta] * 5 / v(M[x,y,1])
            if M[x,y,3] == 1: T_bad += omega[dL][dtheta] * 5 / v(M[x,y,1])
        else: print("此路不通！")
        path_coordinates.append((y,x))
        s = (M[x,y,1] + M[xp,yp,1]) / 2
        p = phi(M[x,y], M[xp,yp])
        stable += s * p
        Dset.append([T, D, M[x,y,0], M[x,y,1], v(M[x,y,1])])
    print(f"路径里程为: {D}\n时效性指标:路径时间为: {T}\n稳定性指标为: {stable}\n安全性指标:不良区域时间为: {T_bad}")
    if True: np.save('path_L1_L2', path_coordinates)
    return Dset, D, T, stable, T_bad

def plot(data, D, T, stable, T_bad):
    # 全局配置
    plt.rcParams['font.family'] = 'SimSun'
    plt.rcParams['font.size'] = 12
    plt.rcParams['axes.unicode_minus'] = False
    black = '#000000'
    plt.rcParams['axes.prop_cycle'] = cycler(color=[black, black, black, black])
    blue='#6495ED'
    red='#FF6347'
    green='#32CD32'
    yellow='#FFD700'
    fig, axs = plt.subplots(2, 2, figsize=(12, 8))
    axs = axs.flatten() 

    data = np.array(data)
    # 里程-时间
    axs[0].plot(data[:, 0], data[:, 1], linewidth=2, color=yellow, zorder=3)
    axs[0].set_title('里程-时间关系', fontsize=14, fontweight='bold', color=black)
    axs[0].set_xlabel('时间 (s)', fontsize=12, fontweight='bold', color=black)
    axs[0].set_ylabel('里程 (m)', fontsize=12, fontweight='bold', color=black)
    axs[0].grid(True, linestyle='--', alpha=0.5, color="#535353")
    axs[0].tick_params(axis='both', which='both', labelcolor=black)

    # 高程-里程
    axs[1].plot(data[:, 1], data[:, 2], linewidth=2, color=red, zorder=3)
    axs[1].set_title('地形高程变化', fontsize=14, fontweight='bold', color=black)
    axs[1].set_xlabel('里程 (m)', fontsize=12, fontweight='bold', color=black)
    axs[1].set_ylabel('高程 (m)', fontsize=12, fontweight='bold', color=black)
    axs[1].grid(True, linestyle='--', alpha=0.5, color="#535353")
    axs[1].tick_params(axis='both', which='both', labelcolor=black)

    # 速度-里程
    axs[2].plot(data[:, 1], data[:, 4], linewidth=2, color=green, zorder=3)
    axs[2].set_title('速度分布', fontsize=14, fontweight='bold', color=black)
    axs[2].set_xlabel('里程 (m)', fontsize=12, fontweight='bold', color=black)
    axs[2].set_ylabel('速度 (m/s)', fontsize=12, fontweight='bold', color=black)
    axs[2].grid(True, linestyle='--', alpha=0.5, color="#535353")
    axs[2].tick_params(axis='both', which='both', labelcolor=black)

    # 坡度-里程
    axs[3].plot(data[:, 1], data[:, 3], linewidth=2, color=blue, zorder=3)
    axs[3].set_title('道路坡度变化', fontsize=14, fontweight='bold', color=black)
    axs[3].set_xlabel('里程 (m)', fontsize=12, fontweight='bold', color=black)
    axs[3].set_ylabel('坡度 (°)', fontsize=12, fontweight='bold', color=black)
    axs[3].grid(True, linestyle='--', alpha=0.5, color="#535353")
    axs[3].tick_params(axis='both', which='both', labelcolor=black)

    # 统一美化
    for ax in axs:
        for spine in ax.spines.values():
            spine.set_color("#535353")
        ax.tick_params(length=0)
    plt.tight_layout(rect=[0, 0.03, 1, 0.95])

    plt.savefig('路径分析_图表.png', dpi=500, bbox_inches='tight')
    plt.show()

# M为预加载数据
M = np.load('map_data.npy')
print(f"地图加载完成：{M.shape[0]}行 × {M.shape[1]}列")
analysis_data, D, T, stable, T_bad = go(M)
plot(analysis_data, D, T, stable, T_bad)